#include "SingleReconstruct.h"

SingleReconstruct::SingleReconstruct()
{

}

SingleReconstruct::~SingleReconstruct()
{

}

void SingleReconstruct::setSensor (SensorDataInput *pSensor)
{
	_pSensor = pSensor;
}

#define FACE_THRESH 1.0


void SingleReconstruct::generateModel (int minDepth, int maxDepth,
		std::vector< Vertex > &vertices,
		std::vector< glm::uvec3 > &indices)
{
	vertices.clear ();
	indices.clear ();

	int frameWidth = _pSensor->getSensorParameter ().frameWidth;
	int frameHeight = _pSensor->getSensorParameter ().frameHeight;
	std::vector< int > fullIndex (frameWidth * frameHeight, -1);

	for (int u = 0, i = 0; u < frameWidth; ++u) {
		for (int v = 0; v < frameHeight; ++v, ++i) {
			float depth = CV_IMAGE_ELEM (_pSensor->getDepthImage (),
					unsigned short, v, u);
			if (depth < minDepth || depth > maxDepth)
				continue;
			Vertex vertex;
			vertex.point = _pSensor->unprojectToWorld (
					glm::vec3 (u, v, depth));
			vertex.texCoord = glm::vec2 ((float)u / (frameWidth - 1),
					1.0f - (float)v / (frameHeight - 1));
			vertex.color = glm::vec3 (CV_IMAGE_ELEM (_pSensor->getRGBImage (),
					unsigned char, v, u * 3 + 2) / 255.0f,
					CV_IMAGE_ELEM (_pSensor->getRGBImage (),
					unsigned char, v, u * 3 + 1) / 255.0f,
					CV_IMAGE_ELEM (_pSensor->getRGBImage (),
					unsigned char, v, u * 3 + 0) / 255.0f);
			vertices.push_back (vertex);
			fullIndex[i] = vertices.size () - 1;
			if (u >= 1 && v >= 1) {
				unsigned int corners[] = {
					fullIndex[i - frameHeight - 1], fullIndex[i - 1],
					fullIndex[i], fullIndex[i - frameHeight]
				};
				if (corners[0] != -1 && corners[1] != -1 && corners[3] != -1 &&
					abs (vertices[corners[0]].point.z - vertices[corners[3]].point.z) < FACE_THRESH &&
					abs (vertices[corners[3]].point.z - vertices[corners[1]].point.z) < FACE_THRESH &&
					abs (vertices[corners[1]].point.z - vertices[corners[0]].point.z) < FACE_THRESH)
					indices.push_back (glm::uvec3 (corners[0], corners[3], corners[1]));
				if (corners[1] != -1 && corners[2] != -1 && corners[3] != -1 &&
					abs (vertices[corners[1]].point.z - vertices[corners[3]].point.z) < FACE_THRESH &&
					abs (vertices[corners[3]].point.z - vertices[corners[2]].point.z) < FACE_THRESH &&
					abs (vertices[corners[2]].point.z - vertices[corners[1]].point.z) < FACE_THRESH)
					indices.push_back (glm::uvec3 (corners[1], corners[3], corners[2]));
			}
		}
	}
	for (int i = 0; i < indices.size (); ++i) {
		glm::vec3 v1 = vertices[indices[i][1]].point - vertices[indices[i][0]].point;
		glm::vec3 v2 = vertices[indices[i][2]].point - vertices[indices[i][1]].point;
		glm::vec3 n = glm::normalize (glm::cross (v1, v2));
		for (int j = 0; j < 3; ++j)
			vertices[indices[i][j]].normal += n;
	}
	for (size_t i = 0; i < vertices.size (); ++i)
		if (vertices[i].normal != glm::vec3 ())
			vertices[i].normal = glm::normalize (vertices[i].normal);
}
